#include "jump_point_search.h"

JumpPointSearchPlanner::JumpPointSearchPlanner(){
    __debug_mode = false;
    // 添加八邻域
    __neighbor_points.emplace_back(cv::Point(-1, 0), 10);
    __neighbor_points.emplace_back(cv::Point(1, 0), 10);
    __neighbor_points.emplace_back(cv::Point(0, -1), 10);
    __neighbor_points.emplace_back(cv::Point(0, 1), 10);
    __neighbor_points.emplace_back(cv::Point(-1, -1), 14);
    __neighbor_points.emplace_back(cv::Point(-1, 1), 14);
    __neighbor_points.emplace_back(cv::Point(1, -1), 14);
    __neighbor_points.emplace_back(cv::Point(1, 1), 14);
}

JumpPointSearchPlanner::~JumpPointSearchPlanner(){

}

// 设置调试模式
void JumpPointSearchPlanner::SetDebugMode(bool mode){
    __debug_mode = mode;
}

// 设置地图
void JumpPointSearchPlanner::SetMap(cv::Mat map){
    __map_mat = map.clone();
    cout <<"[JumpPointSearchPlanner::SetMap] rows=" << __map_mat.rows 
        << ", cols=" << __map_mat.cols 
        << endl;
    int max_size = std::max(__map_mat.rows, __map_mat.cols);
    if(__cost_cache_mat.size() != max_size*max_size){
        cout << "update cost cache matrix size = " << max_size*max_size << endl;
        __cost_cache_mat.resize(max_size, max_size);
        for(int x = 0; x < max_size; x++){
            for (int y = 0; y < max_size; y++){
                __cost_cache_mat(x,y) = hypot(x, y) * 10;
            }
        }
    }
}

// 计算到起点代价
float JumpPointSearchPlanner::CalCostG(cv::Point point, cv::Point start, float D){
    return D;
}

// 计算到终点代价
float JumpPointSearchPlanner::CalCostH(cv::Point point, cv::Point goal, float D){
    int dx = abs(goal.x - point.x);
    int dy = abs(goal.y - point.y);
    // return hypot(dx, dy) * 10;
    return __cost_cache_mat(dx, dy);
    
}

// 计算点是否有效
bool JumpPointSearchPlanner::IsValid(cv::Point point){
    if(point.x < 0 || point.y < 0){
        return false;
    }
    if(point.x >= __map_mat.cols || point.y >= __map_mat.rows){
        return false;
    }
    // 是否在障碍物中
    if(__map_mat.at<uchar>(point) < 200){
        return false;
    }
    return true;
}

// 判断两点是否相等
bool JumpPointSearchPlanner::IsEqual(cv::Point point1, cv::Point point2){
    if(point1.x == point2.x && point1.y == point2.y){
        return true;
    }
    return false;
}


// 规划路径
vector<cv::Point> JumpPointSearchPlanner::GetPath(cv::Point start, cv::Point goal, 
    vector<cv::Point>& expand_points, vector<cv::Point>& jump_points){
    vector<cv::Point> path_points;
    cout << "[JumpPointSearchPlanner::GetPath] start=(" 
        << start.x << ", " << start.y << "), goal=("
        << goal.x << ", " << goal.y << "), map rows = "
        << __map_mat.rows << ", cols = " << __map_mat.cols 
        << endl;

    
    // 初始化数据
    std::priority_queue<GP*, vector<GP*>, GPcmp> __open_list;
    std::unordered_map<string, GP*> __closed_list;

    // 起点放入open list
    GP* start_node = new GP(start);
    __open_list.push(start_node);

    // 开始计算
    __debug_count = 0;
    while(true && !__open_list.empty()){
        // 获取优先级最高的点 __open_list
        GP* current_node = __open_list.top(); 

        // 删除当前点
        __open_list.pop();
        if (__closed_list.find(current_node->GetId()) != __closed_list.end())
            continue;
        __closed_list.insert(std::make_pair(current_node->GetId(), current_node));

        if(IsEqual(current_node->GetPoint(), goal)){
            // 找到终点，退出循环
            cout << "[JumpPointSearchPlanner::GetPath] find goal. open_list size: " << __open_list.size() << endl;
            while(current_node->GetParent() != nullptr){
                path_points.emplace_back(current_node->GetPoint());
                current_node = current_node->GetParent();
            }
            path_points.emplace_back(current_node->GetPoint());
            return path_points;
        }

        // __debug_count ++;
        // if(__debug_count > 5){
        //     while(current_node->GetParent() != nullptr){
        //         path_points.emplace_back(current_node->GetPoint());
        //         current_node = current_node->GetParent();
        //     }
        //     path_points.emplace_back(current_node->GetPoint());
        //     return path_points;
        // }

        // 遍历临近节点
        int idx = 0;
        for(auto& point_data : __neighbor_points){
            // idx++;
            // cout "idx: " << idx << endl;
            // if(idx > 8){
            //     return path_points;
            // } 
            cv::Point jump_point = cv::Point(-1, -1);
            jump_point = JumpPointSearch(current_node->GetPoint(), point_data.first, goal, expand_points);
            if(IsEqual(jump_point, cv::Point(-1, -1)))
                continue;

            GP* new_node = new GP(jump_point, 
                current_node->GetCostG() + CalCostG(jump_point, start, point_data.second), 
                // CalCostG(jump_point, start, point_data.second), 
                CalCostH(jump_point, goal, point_data.second)
            );
 
            // 检查点是否有效
            if(!IsValid(jump_point)){
                continue;
            }
            // 检查点是否已经访问过
            if (__closed_list.find(new_node->GetId()) != __closed_list.end())
                continue;

            cout << "[JumpPointSearchPlanner::GetPath] jump_point = ("
                << jump_point.x << ", " << jump_point.y 
                << ", open_list size: " << __open_list.size()
                << endl;

            // 放入open list
            new_node->SetParent(current_node);
            __open_list.push(new_node);
            jump_points.emplace_back(jump_point);
        }
    }
    
    return path_points;
}

// 计算跳点
cv::Point JumpPointSearchPlanner::JumpPointSearch(cv::Point curr_point, cv::Point dir_point, cv::Point goal, vector<cv::Point>& expand_points){
    cv::Point jump_point = cv::Point(-1, -1);
    auto new_point = curr_point + dir_point;
    expand_points.emplace_back(new_point);

    // 检查点是否有效
    if(!IsValid(new_point)){
        return cv::Point(-1, -1);
    }

    // 检查点是否为终点
    if(IsEqual(new_point, goal)){
        cout << "[JumpPointSearch] new_point = (" << new_point.x << ", " << new_point.y << ") is goal" << endl;
        return new_point;
    }

    // 判断方向
    int x_dir = dir_point.x;
    int y_dir = dir_point.y;
    
    // cout << "[JumpPointSearch] x_dir = " << x_dir << ", y_dir = " << y_dir 
    //     << ", new_point = (" << new_point.x << ", " << new_point.y << ")" << endl;

    // 检测强制邻居是否存在
    if(DetectForceNeighbor(new_point, dir_point)){
        return new_point;
    }

    // 检测对角线的水平和垂直方向是否存在跳点
    if(x_dir && y_dir){
        cv::Point new_dir_point;
        // 水平线
        new_dir_point = cv::Point(x_dir, 0);
        jump_point = JumpPointSearch(new_point, new_dir_point, goal, expand_points);
        // if(!IsEqual(jump_point, cv::Point(-1, -1))) return jump_point;
        if(!IsEqual(jump_point, cv::Point(-1, -1))) return new_point;
        // 垂直线
        new_dir_point = cv::Point(0, y_dir);
        jump_point = JumpPointSearch(new_point, new_dir_point, goal, expand_points);
        // if(!IsEqual(jump_point, cv::Point(-1, -1))) return jump_point;
        if(!IsEqual(jump_point, cv::Point(-1, -1))) return new_point;
    }

    // cout << "[JumpPointSearch][" << __debug_count << "]-------------------------------------end" << endl;
    // 进行下一次递归调用
    return JumpPointSearch(new_point, dir_point, goal, expand_points);
}


// 检测强制邻居是否存在
bool JumpPointSearchPlanner::DetectForceNeighbor(cv::Point curr_point, cv::Point dir_point){
    // 判断方向
    int x_dir = dir_point.x;
    int y_dir = dir_point.y;

    // horizontal 水平方向
    if(x_dir && !y_dir){
        if(!IsValid(cv::Point(curr_point.x, curr_point.y + 1)) 
            && IsValid(cv::Point(curr_point.x + x_dir, curr_point.y + 1))){
            // cout << "[DetectForceNeighbor] horizontal 1 ." << endl;
            return true;
        }
        if(!IsValid(cv::Point(curr_point.x, curr_point.y - 1)) 
            && IsValid(cv::Point(curr_point.x + x_dir, curr_point.y - 1))){
            // cout << "[DetectForceNeighbor] horizontal 2 ." << endl;
            return true;
        }
    }

    // vertical 垂直方向
    if(!x_dir && y_dir){
        if(!IsValid(cv::Point(curr_point.x + 1, curr_point.y)) 
            && IsValid(cv::Point(curr_point.x + 1, curr_point.y + y_dir))){
            // cout << "[DetectForceNeighbor] vertical 1 ." << endl;
            return true;
        }
        if(!IsValid(cv::Point(curr_point.x - 1, curr_point.y)) 
            && IsValid(cv::Point(curr_point.x - 1, curr_point.y + y_dir))){
            // cout << "[DetectForceNeighbor] vertical 2 ." << endl;
            return true;
        }
    }

    // diagonal 对角线
    if(x_dir && y_dir){
        if(!IsValid(cv::Point(curr_point.x - x_dir, curr_point.y)) 
            && IsValid(cv::Point(curr_point.x - x_dir, curr_point.y + y_dir))){
            // cout << "[DetectForceNeighbor] diagonal 1 ." << endl;
            return true;
        }
        if(!IsValid(cv::Point(curr_point.x, curr_point.y - y_dir)) 
            && IsValid(cv::Point(curr_point.x + x_dir, curr_point.y - y_dir))){
            // cout << "[DetectForceNeighbor] diagonal 2 ." << endl;
            return true;
        }
    }

    return false;
}




